/*
 * wallfollow.cpp
 *
 *  Created on: Oct 22, 2012
 *      Author: robot
 */
#include <behavior/wallfollow.h>


Wallfollow::Wallfollow (ros::NodeHandle n) :
	nodeHandle(n)
{
	priority   	= 0.1;
	active 		= true;
	subscriber 	= nodeHandle.subscribe("/sensor/ir", 5, & Wallfollow::distance_callback, this);
	ROS_INFO("Wall following behavior ready.");
}


void Wallfollow::distance_callback (const sensor::IR::ConstPtr & msg)
{
	leftFront	= msg -> dists[0];
	leftBack 	= msg -> dists[1];
	rightFront  = msg -> dists[5];
	rightBack   = msg -> dists[4];

	// Normalize values.
	if (leftFront < 0.01 || leftFront > 0.30)
		leftFront = 0.30;
	if (leftBack < 0.01 || leftBack > 0.30)
		leftBack = leftFront;
	if (rightFront < 0.01 || rightFront > 0.30)
		rightFront = 0.30;
	if (rightBack < 0.01 || rightBack > 0.30)
		rightBack = rightFront;

	left 	= (leftFront  + leftBack)  / 2;
	right 	= (rightFront + rightBack) / 2;

	float variance 	= (leftFront > leftBack)   ? (leftBack / leftFront)   : (leftFront / leftBack);

	priority = 0.1;
	if (variance < 0.9 && leftFront < 0.30)
		priority = 0.4;
	if (variance < 0.6 && leftFront < 0.30)
		priority = 0.6;

	// Too close to a wall. Also act.
	if ((left < 0.09 && left > 0.01) || (right < 0.08 && right > 0.01))
		priority = 0.4;
}


bool Wallfollow::act (ros::Publisher publisher)
{
	driver::Move move;
	move.wait_until_completion = false;

	float angle 	= (left < right) ? (leftBack  - leftFront) : (rightFront - rightBack);

	move.distance 	= 0.0;
	move.angle 		= -2.5 * angle;

	// Too close to the wall. Move off a bit.
	if (left < 0.09 && left > 0.01) {
		move.angle 	  = -0.1;
		move.distance =  0.05;
	}
	if (right < 0.08 && right > 0.01) {
		move.angle 	  =  0.1;
		move.distance =  0.05;
	}
	ROS_INFO("Wall-following sending command: %f, %f", move.distance, move.angle);
	publisher.publish(move);
	priority = 0.1;
	return true;
}


